#include "vision_service_direct_funcs.h"
#include "vision_service_direct_types.h"

// #include <opencv2/opencv.hpp>

#include <unistd.h>
#include <fstream>
#include <iostream>


#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/NavSatFix.h>
#include <camera_info_manager/camera_info_manager.h>

// #include <sstream>
// #include <tf/transform_broadcaster.h>
// #include <message_filters/subscriber.h>
#include <sensor_msgs/image_encodings.h>
#include <std_msgs/Int16.h>
#include <std_msgs/Int16.h>
#include <zhz_msgs/FCVisionSwitchFrame.h>
#include <zhz_msgs/NedFrame.h>
#include <zhz_msgs/RCFrame.h>


// Publish ImageFrame data as a ROS Image message
sensor_msgs::Image ImageFrameToROSImg(const ImageFrame& image_frame)
{
    sensor_msgs::Image image_msg;

    image_msg.header.stamp = ros::Time(image_frame.timestamp / 1000000000, image_frame.timestamp % 1000000000);
    image_msg.header.frame_id = "camera_frame";

    image_msg.height = image_frame.rows;
    image_msg.width = image_frame.cols;
    image_msg.step = image_frame.step;

    switch (image_frame.color) {
        case COLOR_GRAY:
            image_msg.encoding = "mono8";
            break;
        case COLOR_YUV:
            image_msg.encoding = "yuv422";
            break;
        case COLOR_BGR:
            image_msg.encoding = "bgr8";
            break;
        default:
            image_msg.encoding = "mono8";  // Default to mono8 if unknown
    }

    // if (image_frame.data != nullptr && image_frame.data[0] != '\0') {
    if (image_frame.data != nullptr) {
      image_msg.data.assign(image_frame.data, image_frame.data + (image_frame.step * image_frame.rows));
    }

    return image_msg;
}

int main(int argc, char** argv) {
  ros::init(argc, argv, "msg_convert_img");
  ros::NodeHandle nh("~");

  std::string left_topic, right_topic;
  std::string camera_info_url_l, camera_info_url_r, camera_info_l, camera_info_r;
  int use_cam_type = 4;
  ros::param::get("~use_cam_type", use_cam_type);
  ros::param::get("~left_topic", left_topic);
  ros::param::get("~right_topic", right_topic);
  ros::param::get("~camera_info_url_l", camera_info_url_l);
  ros::param::get("~camera_info_url_r", camera_info_url_r);
  ros::param::get("~camera_info_l", camera_info_l);
  ros::param::get("~camera_info_r", camera_info_r);
  boost::shared_ptr<camera_info_manager::CameraInfoManager> cinfo_l;
  boost::shared_ptr<camera_info_manager::CameraInfoManager> cinfo_r;
  ros::Publisher img1_pub = nh.advertise<sensor_msgs::Image>(left_topic, 1, true);
  ros::Publisher img2_pub = nh.advertise<sensor_msgs::Image>(right_topic, 1, true);

  cinfo_l.reset(new camera_info_manager::CameraInfoManager(nh, "camera", camera_info_url_l));
  ros::Publisher camera_info_pub_l_ = nh.advertise<sensor_msgs::CameraInfo>(camera_info_l, 1);
  ros::Publisher camera_info_pub_r_ = nh.advertise<sensor_msgs::CameraInfo>(camera_info_r, 1);
  sensor_msgs::CameraInfo camera_info_msg_l = cinfo_l->getCameraInfo();
  cinfo_r.reset(new camera_info_manager::CameraInfoManager(nh, "camera", camera_info_url_r));
  sensor_msgs::CameraInfo camera_info_msg_r = cinfo_r->getCameraInfo();
  // sensor_msgs::CameraInfoPtr camera_info_msg_l(new sensor_msgs::CameraInfo(cinfo_l->getCameraInfo()));
  // sensor_msgs::CameraInfoPtr camera_info_msg_r(new sensor_msgs::CameraInfo(cinfo_r->getCameraInfo()));
  std::cout << "Node use_cam_type: " << use_cam_type << std::endl;


  ros::Rate rate(25);
  int frame_id = 0;
  int ret = 0;
  // 1. open msg receiver
  ret = vision_create_device_stream();  // 0: success; -1: failed
  while(ros::ok()) {
    if (ret) {
        printf("vision_create_device_stream failed.\n");
        rate.sleep();
        continue;
    }
      // 2. Get Stereo Images
    ImageFrame img1, img2;
    // 数据集的0，1是前，2，3是后，4，5是下
    // DEVICE_STEREO_CAMERA_HEAD = 0,
    // DEVICE_STEREO_CAMERA_REAR,
    // DEVICE_STEREO_CAMERA_BOTTOM,
    // DEVICE_STEREO_CAMERA_RIGHT,
    // DEVICE_STEREO_CAMERA_LEFT,
    // DEVICE_STEREO_CAMERA_TOP,
    if (use_cam_type == 0 || use_cam_type == 1) {
      ret = vision_get_stereo_camera_distorted_frame(DeviceStream::DEVICE_STEREO_CAMERA_HEAD, MemoryType::MEMORY_VIRT,
                                                      &img1, &img2);   
    } else if (use_cam_type == 2 || use_cam_type == 3) {
      ret = vision_get_stereo_camera_distorted_frame(DeviceStream::DEVICE_STEREO_CAMERA_REAR, MemoryType::MEMORY_VIRT,
                                                      &img1, &img2);   
    } else {
      ret = vision_get_stereo_camera_distorted_frame(DeviceStream::DEVICE_STEREO_CAMERA_BOTTOM, MemoryType::MEMORY_VIRT,
                                                      &img1, &img2);   
    }
    sensor_msgs::Image img1_msg = ImageFrameToROSImg(img1);
    sensor_msgs::Image img2_msg = ImageFrameToROSImg(img2);
    img1_msg.header.seq =  frame_id;
    img2_msg.header.seq =  frame_id++;
    img1_pub.publish(img1_msg);
    img2_pub.publish(img2_msg);

    camera_info_msg_l.header =img1_msg.header;
    camera_info_pub_l_.publish(camera_info_msg_l);
    camera_info_msg_r.header.frame_id = img2_msg.header.frame_id;
    camera_info_pub_r_.publish(camera_info_msg_r);

    ros::spinOnce();
    rate.sleep();
  }
  ros::shutdown();
  exit(0);
  return 0;
}